#include "motor_control.h"
#include "HT03_driver.h"
#include "interrupt_manager.h"

#define MOTORS_SIZE 8

HT_t motors[MOTORS_SIZE]; // 八个电机
MotorParam_t param;

void motor_control_init(void)
{
    /* 初始化CAN和电机ID */
    can_filterinit(&hcan1);
    can_filterinit(&hcan2);
    attachinterrupt_can(&hcan1, HT_can_callback);
    attachinterrupt_can(&hcan2, HT_can_callback);
    HT_init(&motors[0], 1, &hcan2);
    HT_init(&motors[1], 2, &hcan2);
    HT_init(&motors[2], 3, &hcan1);
    HT_init(&motors[3], 4, &hcan1);
    HT_init(&motors[4], 5, &hcan1);
    HT_init(&motors[5], 6, &hcan1);
    HT_init(&motors[6], 7, &hcan2);
    HT_init(&motors[7], 8, &hcan2);
}

void motor_control_run(void)
{
    for (size_t i = 0; i < MOTORS_SIZE; i++)
    {
        HT_run(&motors[i]);
    }
}

inline void motor_set_param(uint8_t id, MotorParam_t *param)
{
    motors[id].target_kp = param->kp;
    motors[id].target_kd = param->kd;
    motors[id].target_torque = param->torque;
    motors[id].target_velocity = param->velocity;
    motors[id].target_position = param->position;
}
inline void motor_set_kp(uint8_t id, float kp)
{
    motors[id].target_kp = kp;
}
inline void motor_set_kd(uint8_t id, float kd)
{
    motors[id].target_kd = kd;
}
inline void motor_set_torque(uint8_t id, float torque)
{
    motors[id].target_torque = torque;
}
inline void motor_set_velocity(uint8_t id, float velocity)
{
    motors[id].target_velocity = velocity;
}
inline void motor_set_position(uint8_t id, float position)
{
    motors[id].target_position = position;
}
inline float motor_get_position(uint8_t id)
{
    return motors[id].position;
}
inline float motor_get_velocity(uint8_t id)
{
    return motors[id].velocity;
}
inline float motor_get_torque(uint8_t id)
{
    return motors[id].torque;
}
